from constants import MAP_HEIGHT, MAP_WIDTH, TILE_SIZE
from helpers import print_1d_map

class Pathfinder():
    """ A* Algoritm for finding shortest path
    http://www.policyalmanac.org/games/aStarTutorial.htm
    http://theory.stanford.edu/~amitp/GameProgramming/
    
    Python Basics: http://www.astro.ufl.edu/~warner/prog/python.html
    """
    
    openList = list()
    closedList = list()
    path = list()
    
    #tileSize = 50                   # Size of one map tile in pixels
    #mapWidth = 80                   # Map width in tiles
    #mapHeight = 80                  # Map height in tiles
    
    
    def __init__(self):
        self.mapWidth = MAP_WIDTH//TILE_SIZE
        self.mapHeight = MAP_HEIGHT//TILE_SIZE
        self.tileSize = TILE_SIZE
        
        
        self.map = [None]*self.mapWidth*self.mapHeight
        self.hcost = [None]*self.mapWidth*self.mapHeight
        self.gcost = [None]*self.mapWidth*self.mapHeight
        self.fcost = [None]*self.mapWidth*self.mapHeight
        self.parent = [None]*self.mapWidth*self.mapHeight
        
        #for i in range(len(self.map)):
        #    self.map[i] = dict(walkability=1)
    
    def find_path(self, start, target):
        '''
        Calculate shortest path. Returns 1 if path is found and 2 if no path is found
        If a successfull path was found a list of nodes can be gotten from the path variable
        '''
        
        print_1d_map(self.map)
        
        startX = start[0]
        startY = start[1]
        targetX = target[0]
        targetY = target[1]
        
        #startX = startX//self.tileSize
        #startY = startY//self.tileSize 
        startId = self.c2i(startX, startY)   
        #targetX = targetX//self.tileSize
        #targetY = targetY//self.tileSize        
        targetId = self.c2i(targetX, targetY)
        
        # If we start at the goal position, return path found
        if startX == targetX and startY == targetY:
            return 1
        
        # If we start at an unwalkable tile, return no path exists
        if self.map[startId] == 0:
            return 2        
        
        # If the goal is an unwalkable tile, return no path exists
        if self.map[targetId] == 0:
            return 2
        
        # Reset some variables
        self.openList = list()
        self.closedList = list()
        
        # Add the start node and set some basic information on it (needed to prevent "Out Of Bounds" errors)
        self.openList.append(startId)
        self.gcost[startId] = 0
        self.hcost[startId] = 0
        self.fcost[startId] = 0
        self.parent[startId] = None        
        #self.map[startId]['Gcost'] = 0
        #self.map[startId]['Hcost'] = 0
        #self.map[startId]['Fcost'] = 0
        #self.map[startId]['parent'] = None
        
        while True:
            if not len(self.openList) == 0:
                #Get the next node to investigate
                parentId = self.openList.pop(0)
                self.closedList.append(parentId)
                
                if parentId == targetId:
                    break
                
                parentX, parentY = self.i2c(parentId)
                #Check all adjacent nodes
                for childY in range(parentY-1, parentY+2):
                    for childX in range(parentX-1, parentX+2):
                        
                        childId = self.c2i(childX, childY)
                        
                        # Skip the node if we are out of the map boundaries
                        if childY < 0 or childX < 0 or childY > self.mapHeight-1 or childX > self.mapWidth-1:
                            continue
                        
                        # Don't search the current node
                        if (childX == parentX and childY == parentY):
                            continue
                        
                        # Skip the node if it is unpassable
                        if self.map[childId] == 0:
                            continue
                        
                        # If the node is in the closedList, skip it
                        if childId in self.closedList:
                            continue
                        
                        # If the node is not in the openList, add it and make some calculations
                        if not childId in self.openList:
                            
                            # Figure out its G cost
                            if abs(childX - parentX) == 1 and abs(childY - parentY) == 1:
                                addedGCost = 14 # Cost of going to diagonal squares    
                            else:
                                addedGCost = 10 # Cost of going to non-diagonal squares                
                            
                            # Calculate necessary values
                            self.gcost[childId] = self.gcost[parentId] + addedGCost
                            self.hcost[childId] = 10*(abs(childX - targetX) + abs(childY - targetY))
                            self.fcost[childId] = self.gcost[childId] + self.hcost[childId]
                            self.parent[childId] = parentId
                            #self.map[childId]['Gcost'] = self.map[parentId]['Gcost'] + addedGCost
                            #self.map[childId]['Hcost'] = 10*(abs(childX - targetX) + abs(childY - targetY))
                            #self.map[childId]['Fcost'] = self.map[childId]['Gcost'] + self.map[childId]['Hcost']
                            #self.map[childId]['parent'] = parentId

                            # Add the new node to the openList and sort it
                            self.openList.append(childId)
                            self.sort_list()
                            #self.openList.sort(key=lambda x: operator.getitem(self.map, 'Fcost'), reverse=False)
                        
                        # If the node is on the openList, recalculate costs
                        else:
                            # Figure out its  new G cost
                            if abs(childX - parentX) == 1 and abs(childY - parentY) == 1:
                                addedGCost = 14 # cost of going to diagonal squares    
                            else:
                                addedGCost = 10 #cost of going to non-diagonal squares                
                            
                            # Calculate necessary values
                            tmpGcost = self.gcost[parentId] + addedGCost
                            if tmpGcost < self.gcost[childId]:
                                self.parent[childId] = parentId
                                self.fcost[childId] = self.gcost[childId] + self.hcost[childId]
                                self.sort_list()
                                #self.openList.sort(key=lambda x: operator.getitem(self.map, 'Fcost'), reverse=False)
                            
                        
                        
                
                
            else:
                return 2
            
        # A path was found! Time to reconstruct it
        nextParent = targetId
        while True:
            self.path.append(nextParent)
            #nextParent = self.map[nextParent]['parent']
            nextParent = self.parent[nextParent]
            if nextParent == startId:
                self.path.append(nextParent)
                break
        
        
        for i in range(len(self.path)):
            x, y = self.i2c(self.path[i])
            x = x*self.tileSize + self.tileSize//2
            y = y*self.tileSize + self.tileSize//2
            self.path[i] = (x, y)
        
        self.path.reverse()
        
        #print (self.path)
        return 1
        
        
    def c2i(self, x, y):
        """Convert from x,y coordinates to id"""
        return x + y*self.mapWidth
    
    def i2c(self, id):
        """Convert from id to x,y coordinates"""
        x = id%self.mapWidth
        y = id//self.mapWidth
        return x, y
    
    def sort_list(self):
        """Bubble sort of the openList, smaller items bubbles to the top"""
        for passesLeft in range(len(self.openList)-1, 0, -1):
            for i in range(passesLeft):
                #if self.map[self.openList[i]]['Fcost'] > self.map[self.openList[i + 1]]['Fcost']:
                if self.fcost[self.openList[i]] > self.fcost[self.openList[i + 1]]:    
                    self.openList[i], self.openList[i + 1] = self.openList[i + 1], self.openList[i]